
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <list>
#include "xrtp_depack.h"
#include "xcutil.h"


struct VP8PayloadDescriptor
{
    bool extendedControlBitsPresent;
    bool nonReferencePicture;
    bool startOfPartition;
    uint8_t partitionIndex;
    bool pictureIdPresent;
    bool temporalLevelZeroIndexPresent;
    bool temporalLayerIndexPreset;
    bool keyIndexPresent;
    bool layerSync;
    uint16_t pictureId;
    uint8_t temporalLevelZeroIndex;
    uint8_t temporalLayerIndex;
    uint8_t keyIndex;
    
    VP8PayloadDescriptor()
    {
        //Empty
        extendedControlBitsPresent = 0;
        nonReferencePicture = 0;
        startOfPartition = 0;
        partitionIndex = 0;
        pictureIdPresent = 0;
        temporalLevelZeroIndexPresent = 0;
        temporalLayerIndexPreset = 0;
        keyIndexPresent = 0;
        layerSync = 0;
        pictureId = 0;
        temporalLevelZeroIndex = 0;
        temporalLayerIndex = 0;
        keyIndex = 0;
    }
    
    VP8PayloadDescriptor(bool startOfPartition,uint8_t partitionIndex)
    {
        //Empty
        extendedControlBitsPresent = 0;
        nonReferencePicture = 0;
        startOfPartition = 0;
        partitionIndex = 0;
        pictureIdPresent = 0;
        temporalLevelZeroIndexPresent = 0;
        temporalLayerIndexPreset = 0;
        keyIndexPresent = 0;
        layerSync = 0;
        pictureId = 0;
        temporalLevelZeroIndex = 0;
        temporalLayerIndex = 0;
        keyIndex = 0;
        //Set values
        this->startOfPartition = startOfPartition;
        this->partitionIndex = partitionIndex;
    }
    
    uint32_t GetSize()
    {
        //Base size
        uint32_t len = 1;
        if (extendedControlBitsPresent)
        {
            len++;
            if (pictureIdPresent)
            {
                if (pictureId>7)
                    len+=2;
                else
                    len+=1;
            }
            if (temporalLevelZeroIndexPresent)
                len++;
            if (temporalLayerIndexPreset || keyIndexPresent)
                len++;
        }
        return len;
    }
    
    uint32_t Parse(const uint8_t* data, uint32_t size)
    {
        uint32_t len = 1;
        //Read first
        extendedControlBitsPresent	= data[0] >> 7;
        nonReferencePicture		= data[0] >> 5 & 0x01;
        startOfPartition		= data[0] >> 4 & 0x01;
        partitionIndex			= data[0] & 0x0F;
        
        //check if more
        if (extendedControlBitsPresent)
        {
            //Read second
            pictureIdPresent		= data[1] >> 7;
            temporalLevelZeroIndexPresent	= data[1] >> 6 & 0x01;
            temporalLayerIndex		= data[1] >> 5 & 0x01;
            keyIndex			= data[1] >> 4 & 0x01;
            //Increase len
            len++;
            
            //Check if we need to read picture id
            if (pictureIdPresent)
            {
                //Check mark
                if (data[len] & 0x80)
                {
                    //Two bytes
                    pictureId = (data[len] & 0x7F) << 8 | data[len+1];
                    //Inc len
                    len+=2;
                } else {
                    //One byte
                    pictureId = data[len];
                    //Inc len
                    len++;
                }
            }
            //check if present
            if (temporalLevelZeroIndexPresent)
            {
                //read it
                temporalLevelZeroIndex = data[len];
                //Inc len
                len++;
            }
            //Check present
            if (temporalLayerIndexPreset || keyIndexPresent)
            {
                //read it
                temporalLevelZeroIndex	= data[len] >> 6;
                layerSync		= data[len] >> 5 & 0x01;
                keyIndex		= data[len] & 0x1F;
                //Inc len
                len++;
            }
        }
        
        return len;
    }
    
    uint32_t Serialize(uint8_t *data,uint32_t size)
    {
        //Check size
        if (size<GetSize())
            //Error
            return 0;
        
        /*
         The first octets after the RTP header are the VP8 payload descriptor,
         with the following structure.
         
		       0 1 2 3 4 5 6 7
         +-+-+-+-+-+-+-+-+
         |X|R|N|S|PartID | (REQUIRED)
         +-+-+-+-+-+-+-+-+
         X: |I|L|T|K|RSV-A  | (OPTIONAL)
         +-+-+-+-+-+-+-+-+
         I: |   PictureID   | (OPTIONAL 8b/16b)
         +-+-+-+-+-+-+-+-+
         L: |   TL0PICIDX   | (OPTIONAL)
         +-+-+-+-+-+-+-+-+
         T: |TID|Y|  RSV-B  | (OPTIONAL)
         +-+-+-+-+-+-+-+-+
         */
        
        data[0] = extendedControlBitsPresent;
        data[0] = data[0]<<2 | nonReferencePicture;
        data[0] = data[0]<<1 | startOfPartition;
        data[0] = data[0]<<4 | (partitionIndex & 0x0F);
        
        //If nothing more present
        if (!extendedControlBitsPresent)
            //Exit now
            return 1;
        data[1] = pictureIdPresent;
        data[1] = data[1] << 1 | temporalLevelZeroIndexPresent;
        data[1] = data[1] << 1 | temporalLayerIndexPreset;
        data[1] = data[1] << 1 | keyIndex;
        data[1] = data[1] << 4;
        
        uint32_t len = 2;
        
        //Check if picture id present
        if (pictureIdPresent)
        {
            //Check long is the picture id
            if (pictureId>>7)
            {
                //Set picture id
                data[len]  = pictureId>>8 | 0x80;
                data[len+1] = pictureId & 0x7F;
                //Inc lenght
                len+=2;
            } else {
                //Set picture id
                data[len] = pictureId & 0x7F;
                //Inc lenght
                len++;
            }
        }
        
        if (temporalLevelZeroIndexPresent)
        {
            //Set picture id
            data[len] = temporalLevelZeroIndexPresent;
            //Inc lenght
            len++;
        }
        
        if (temporalLayerIndex || keyIndexPresent)
        {
            //Set picture id
            data[len] = temporalLayerIndex;
            data[len] = data[len]<<2 | layerSync;
            data[len] = data[len]<<1 | (keyIndex & 0x1F);
            //Inc lenght
            len++;
        }
        
        return len;
    }
    
//    void Dump()
//    {
//        Debug("[VP8PayloadDescriptor \n");
//        Debug("\t extendedControlBitsPresent=%d\n"	, extendedControlBitsPresent);
//        Debug("\t nonReferencePicture=%d\n"		, nonReferencePicture);
//        Debug("\t startOfPartition=%d\n"		, startOfPartition);
//        Debug("\t partitionIndex=%d\n"			, partitionIndex);
//        Debug("\t pictureIdPresent=%d\n"		, pictureIdPresent);
//        Debug("\t temporalLevelZeroIndexPresent=%d\n"	, temporalLevelZeroIndexPresent);
//        Debug("\t temporalLayerIndexPreset=%d\n"	, temporalLayerIndexPreset);
//        Debug("\t keyIndexPresent=%d\n"			, keyIndexPresent);
//        Debug("\t layerSync=%d\n"			, layerSync);
//        Debug("\t pictureId=%u\n"			, pictureId);
//        Debug("\t temporalLevelZeroIndex=%d\n"		, temporalLevelZeroIndex);
//        Debug("\t temporalLayerIndex=%d\n"		, temporalLayerIndex);
//        Debug("\t keyIndex=%d\n"			, keyIndex);
//        Debug("/]\n");
//    }
};

class XRTPPayloadParser{
public:
    XRTPPayloadParser():isKeyFrame_(false),isStartOfFrame_(false){}
    virtual ~XRTPPayloadParser(){};
    virtual int parse(const uint8_t * payload, int length) = 0;
    bool isKeyFrame() {return this->isKeyFrame_;}
    bool isStartOfFrame() {return this->isStartOfFrame_;}
    static XRTPPayloadParser * getParser(int codec);
protected:
    bool isKeyFrame_;
    bool isStartOfFrame_;
};

class XVP8PayloadParser : public XRTPPayloadParser{
public:
    virtual ~XVP8PayloadParser(){};
    virtual int parse(const uint8_t * payload, int payload_length) {
        VP8PayloadDescriptor desc;
        uint32_t descLen = desc.Parse(payload, payload_length);
        this->isStartOfFrame_ = desc.startOfPartition;
        this->isKeyFrame_ = !(payload[descLen] & 0x01);
        return 0;
    }

};

XRTPPayloadParser * XRTPPayloadParser::getParser(int codec){
    static XVP8PayloadParser * parser = new XVP8PayloadParser();
    return parser;
}

class XRTPItem{
public:
    XRTPItem(const uint8_t * rtpData, int rtpLength
             , const uint8_t * frameData, int frameDataLength
             , bool mask, uint32_t timestamp, uint32_t seq
             , bool isKeyframe, bool isStartOfFrame)
    :rtpData_(rtpData), rtpLength_(rtpLength)
    , frameData_(frameData), frameDataLength_(frameDataLength)
    , mask_(mask), timestamp_(timestamp), seq_(seq)
    , isKeyframe_(isKeyframe), isStartOfFrame_(isStartOfFrame){
    }
    
    const uint8_t * rtpData_;
    int rtpLength_;
    const uint8_t * frameData_;
    int frameDataLength_;
    bool mask_;
    uint32_t timestamp_;
    uint32_t seq_;
    bool isKeyframe_;
    bool isStartOfFrame_;
};


static const uint32_t RTP_SEQ_MOD = (1<<16);
static const uint32_t MAX_MISORDER = 100;

struct XRTPDepacker{
    
    XRTPDepacker(){
        reset();
    }
    
    int addRtp(int codec, const uint8_t * rtpData, int rtpLength){
//        unsigned char v =  (rtpData[0]>>6) & 0x3;
//        unsigned char p =  (rtpData[0]>>5) & 0x1;
        unsigned char x =  (rtpData[0]>>4) & 0x1;
        unsigned char cc = (rtpData[0]>>0) & 0xF;
        unsigned char m =  (rtpData[1]>>7) & 0x1;
//        unsigned char pt = (rtpData[1]>>0) & 0x7F;
        unsigned short seq = be_get_u16(rtpData+2);
        unsigned int ts = be_get_u32(rtpData+4);
//        unsigned int ssrc = be_get_u32(rtpData+8);
        
        int header_len = 12 + cc * 4;
        if(x){
            int min = header_len + 4;
            if(min <= rtpLength){
                int ext_len = be_get_u16(rtpData+header_len+2);
                header_len += 4+ext_len*4;
            }else{
                header_len = -1;
                return -1;
            }
        }
        
        //
        // make seq
        //
        if(lastSeq_ != UINT32_MAX){
            uint32_t distance1 = seq - lastSeq_;
            uint32_t distance2 = lastSeq_ - seq;
            uint32_t distance = distance1 < distance2 ? distance1 : distance2;
            if(distance > MAX_MISORDER){
                reset();
            }else{
                // deals wrapping
                if ((seq & 0xFFFF) < 0x0FFF && (lastSeq_ & 0xFFFF)>0xF000) {
                    cycles_ += RTP_SEQ_MOD;
                }else if((lastSeq_ & 0xFFFF) <0x0FFF && (seq & 0xFFFF)>0xF000) {
                    cycles_ -= RTP_SEQ_MOD;
                }
            }
        }
        lastSeq_ = seq;
        seq = cycles_ + seq;
        
        
        //
        // insert to list
        //
        XRTPPayloadParser * parser = XRTPPayloadParser::getParser(codec);
        parser->parse(rtpData+header_len, rtpLength-header_len);
        insertRtpItem(XRTPItem(rtpData, rtpLength, NULL, 0 , m, ts, seq, parser->isKeyFrame(), parser->isStartOfFrame()));
        
        return 0;
    }
    
    int getNextFrameLength(){
        XRTPItem& firstItem = rtpList_.front();
        if(firstItem.seq_ == nextSeq_){
            return getFrameInOrder();
        }
        
        if(firstItem.isStartOfFrame_){
            cleanFrameState();
            nextSeq_ = firstItem.seq_;
            return getFrameInOrder();
        }
        
        // check list full
        if(rtpList_.size() >= MAX_MISORDER){
            cleanFrameState();
            rtpList_.pop_front(); // drop 1 packet
        }
        return 0;
    }

    
protected:
    int getFrameInOrder(){
        int frameLength = 0;
        while(frameLength == 0){
            XRTPItem& firstItem = rtpList_.front();
            if(firstItem.seq_ != nextSeq_){
                break;
            }
            nextSeq_++;
            if(firstItem.mask_){
                frameLength = 1;
            }
            rtpList_.pop_front();
        }
        return frameLength;
    }
    
    void insertRtpItem(const XRTPItem & newItem){
        
        std::list<XRTPItem>::iterator it;
        for (it=rtpList_.begin(); it!=rtpList_.end(); ++it){
            XRTPItem& itemtemp = *it;
            if(newItem.seq_ < itemtemp.seq_){
                rtpList_.insert(it, newItem);
                break;
            }
        }
        if(it == rtpList_.end()){
            rtpList_.push_back(newItem);
        }
    }
    
    void cleanFrameState(){
        lastCodec_ = -1;
        nextSeq_ = UINT32_MAX;
    }
    
    void reset(){
        cleanFrameState();
        lastSeq_ = UINT32_MAX;
        cycles_ = RTP_SEQ_MOD;
        rtpList_.clear();
    }
    
    
    std::list<XRTPItem> rtpList_;
    int lastCodec_;
    uint32_t nextSeq_;
    uint32_t lastSeq_;
    uint32_t cycles_;
};


